/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008-2012. All Rights Reserved.                        */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/
package teamBlitz;

import com.sun.squawk.security.HexEncoding;
import edu.wpi.first.wpilibj.*;

/**
 *
 * @author dtjones
 */




public class ITG3200_I2C_6DOF extends I2CSync  {

private static final byte ITG_ADDR = (byte)0x66; //0xD0 if tied low, 0xD2 if tied high
private static final byte WHO_AM_I		= 0x00;
private static final byte SMPLRT_DIV	= 0x15;
private static final byte DLPF_FS		= 0x16;
private static final byte INT_CFG		= 0x17;
private static final byte INT_STATUS	= 0x1A;
private static final byte TEMP_OUT_H	= 0x1B;
private static final byte TEMP_OUT_L	= 0x1C;
private static final byte GYRO_XOUT_H	= 0x1D;
private static final byte GYRO_XOUT_L	= 0x1E;
private static final byte GYRO_YOUT_H	= 0x1F;
private static final byte GYRO_YOUT_L	= 0x20;
private static final byte GYRO_ZOUT_H	= 0x21;
private static final byte GYRO_ZOUT_L	= 0x22;
private static final byte PWR_MGM		= 0x3E;   

//Sample Rate Divider
//Fsample = Fint / (divider + 1) where Fint is either 1kHz or 8kHz
//Fint is set to 1kHz
//Set divider to 9 for 100 Hz sample rate
//DLPF, Full Scale Register Bits
//FS_SEL must be set to 3 for proper operation
//Set DLPF_CFG to 3 for 1kHz Fint and 42 Hz Low Pass Filter
private static final byte DLPF_CFG_0	= (1<<0);
private static final byte DLPF_CFG_1	= (1<<1);
private static final byte DLPF_CFG_2	= (1<<2);
private static final byte DLPF_FS_SEL_0	= (1<<3);
private static final byte DLPF_FS_SEL_1	= (1<<4);

//Power Management Register Bits
//Recommended to set CLK_SEL to 1,2 or 3 at startup for more stable clock
private static final byte PWR_MGM_CLK_SEL_0		= (1<<0);
private static final byte PWR_MGM_CLK_SEL_1		= (1<<1);
private static final byte PWR_MGM_CLK_SEL_2		= (1<<2);
private static final byte PWR_MGM_STBY_Z		= (1<<3);
private static final byte PWR_MGM_STBY_Y		= (1<<4);
private static final byte PWR_MGM_STBY_X		= (1<<5);
private static final byte PWR_MGM_SLEEP			= (1<<6);
private static final byte PWR_MGM_H_RESET		= (byte)(1<<7); //Interrupt Configuration Bist
private static final byte INT_CFG_ACTL			= (byte)(1<<7);
private static final byte INT_CFG_OPEN			= (1<<6);
private static final byte INT_CFG_LATCH_INT_EN	= (1<<5);
private static final byte INT_CFG_INT_ANYRD		= (1<<4);
private static final byte INT_CFG_ITG_RDY_EN	= (1<<2);
private static final byte INT_CFG_RAW_RDY_EN	= (1<<0);
 
    private float x,y,z;


    public float getX() {
        return (float) (x/14.375);
    }

    public float getY() {
        return (float) (y/14.375);
    }

    public float getZ() {
        return (float) (z/14.375);
    }
  
    /**
     * Constructor.
     *
     * @param slot The slot of the digital module that the sensor is plugged into.
     */
    public ITG3200_I2C_6DOF(int slot) {
        super(DigitalModule.getInstance(slot),ITG_ADDR);
        System.out.println("TeamBlitz starting Gyro");
//        setCompatabilityMode(true);
        boolean rc=false;
        byte[] expected=new byte[2];
//        expected[0]=ITG_ADDR;
        this.setCompatabilityMode(false);
        Timer.delay(.1);          
             rc=this.addressOnly();
        System.out.println("add only rc="+rc);
        Timer.delay(.1);          

        this.setCompatabilityMode(true);
        Timer.delay(.1);          
             rc=this.addressOnly();
        System.out.println("true add only rc="+rc);
        Timer.delay(.1);          
        do{
        rc=read(WHO_AM_I, 1, expected);
        
        System.out.println("who am I="+HexEncoding.hexEncode(expected,2)+" rc="+rc);
        Timer.delay(.1);          
        }while (expected[0]==0);
        rc=write(PWR_MGM, PWR_MGM_H_RESET);
        System.out.println("PWR_MGM="+rc);    
        Timer.delay(.1);
        //Set internal clock to 1kHz with 42Hz LPF and Full Scale to 3 for proper operation	
        rc=write(DLPF_FS, DLPF_FS_SEL_0|DLPF_FS_SEL_1|DLPF_CFG_0);	
        System.out.println("DLPF_FS="+rc);
        Timer.delay(.1);          
                
        //Set sample rate divider for 100 Hz operation	
        rc=write(SMPLRT_DIV, 9);	//Fsample = Fint / (divider + 1) where Fint is 1kHz	
        System.out.println("SMPLRT_DIV="+rc);
        Timer.delay(.1);          

        //Setup the interrupt to trigger when new data is ready.	
        rc=write(INT_CFG, INT_CFG_RAW_RDY_EN | INT_CFG_ITG_RDY_EN);	
        System.out.println("INT_CFG="+rc);
        Timer.delay(.1);          
        
        //Select X gyro PLL for clock source	
        rc=write(PWR_MGM, PWR_MGM_CLK_SEL_0);
        System.out.println("PWR_MGM="+rc);        
        System.out.println("TeamBlitz started Gyro");
        
    }


    /**
     * update all axes.
     *
     * @return boolean success.
     */
    public boolean update() {
        byte[] input = new byte[2];        
        byte[] rawData = new byte[6];
        for(int i=0; i<6; i++)
        {
            read(GYRO_XOUT_H+i, 1, input);              //Y?
            read(GYRO_YOUT_H+i, 1, input);  
            rawData[i]=input[0];
          //  System.out.println("return" + i + " ="+ rawData[i]);
        }
        x = rawData[1]+(rawData[0]<<8);
        y = rawData[3]+(rawData[2]<<8);
        z = rawData[5]+(rawData[4]<<8);
        return true;
    }
}
